import cv2
import threading
import numpy as np

# 手动输入标定参数
mtx = np.array([[379.7052, 0, 324.8973],  # 内参矩阵 [[fx, 0, cx],
                [0, 381.1496, 223.9433],  # [0, fy, cy],
                [0, 0, 1]])  # [0, 0, 1]]

dist = np.array([0.1409, -0.2681, 0, 0, 0])  # 畸变系数 [k1,k2,p1,p2,k3]


class FrameManager:
    def __init__(self):
        self.current_frame = None
        self.is_running = False
        self.camera_cap = None
        self.thread = None
        self.frame_lock = threading.Lock()
        self.mouse_x = 0
        self.mouse_y = 0

    def start_camera(self, camera_id):
        self.camera_cap = cv2.VideoCapture(camera_id)
        if not self.camera_cap.isOpened():
            return False
        self.is_running = True
        self.thread = threading.Thread(target=self._capture_frames, daemon=True)
        self.thread.start()
        return True

    def _capture_frames(self):
        while self.is_running:
            ret, frame = self.camera_cap.read()
            if ret:
                frame = cv2.resize(frame, (640, 480))  # 调整帧大小
                frame = cv2.undistort(frame, mtx, dist)
                frame = cv2.flip(frame, 0)  # 上下翻转帧
                frame = cv2.flip(frame, 1)
                with self.frame_lock:
                    self.current_frame = frame  # 更新帧

    def get_current_frame(self):
        with self.frame_lock:
            return self.current_frame.copy() if self.current_frame is not None else None

    def stop_camera(self):
        self.is_running = False
        if self.camera_cap:
            self.camera_cap.release()
        cv2.destroyAllWindows()


if __name__ == "__main__":
    frame_manager = FrameManager()
    count = 0
    if frame_manager.start_camera(0):
        try:
            while True:
                frame = frame_manager.get_current_frame()
                if frame is not None:
                    cv2.imshow("Camera Feed", frame)
                key = cv2.waitKey(1)
                if key & 0xFF == ord('q'):
                    break
                if key & 0xFF == ord('s'):
                    # 修正文件路径中的转义字符
                    path = r"E:\Innovative Practice\MyProject\images\hand_" + str(count) + ".jpg"
                    cv2.imwrite(path, frame)
                    count += 1
                    print(f"图片保存为 {path}")
        finally:
            frame_manager.stop_camera()
    else:
        print("Failed to open camera.")
